#include "rt_servo_plan.h"
#include "log/zuclog.h"
#include "kine/kine.h"
#include "axisgroup/pfsm_servo_filter.h"
#include "axisgroup/stop_planner.h"
#include "zuc/nml_intf/zucpos2.h"

#define SERVO_PLAN_INFO(fmt, ...) zuclog_info("SERVO_PLAN %d", fmt, group_id_, ##__VA_ARGS__)
#define SERVO_PLAN_ERR(fmt, ...) zuclog_error("SERVO_PLAN %d", fmt, group_id_, ##__VA_ARGS__)
#define SERVO_PLAN_DEBUG(fmt, ...) zuclog_debug("SERVO_PLAN %d", fmt, group_id_, ##__VA_ARGS__)
axisgroup::RtPlanServo::RtPlanServo(int id)
{
    group_id_ = id;
    servo_filter_ = std::make_shared<axisgroup::ServoFilterIntf>();
    stop_planner_ = std::make_shared<axisgroup::StopPlanner>();
    active_ = false;
    paused_ = false;
    wish_to_stop_ = false;
}
int axisgroup::RtPlanServo::init(std::shared_ptr<kine::KineItf> kine, AxisGroupPose pose, double ts)
{
    if (!kine.get())
    {
        return -1;
    }
    kine_ = kine;
    ts_ = ts;
    cmd_list_.clear();
    SERVO_PLAN_INFO("Initd, filter type is %s", servo_filter_->get_filter_name().c_str());
    SERVO_PLAN_INFO("curr jpos %s || pose %s", pose.jpos.val_str().c_str(), pose.pose_str().c_str());
    last_cmd_ = pose;
    last_user_cmd_rt_.cmd = pose;
    last_cmd_rt_ = pose;
    active_ = false;
    paused_ = false;
    wish_to_stop_ = false;
    return 0;
}

void axisgroup::RtPlanServo::set_active(bool active)
{
    active_ = active;
    SERVO_PLAN_INFO("set_active %d", active);
}

void axisgroup::RtPlanServo::pause(bool en)
{
    paused_ = true;
    wish_to_stop_ = true;
    is_stopped_ = false;
}

int axisgroup::RtPlanServo::update(AxisGroupPose desired, AxisGroupPose& target)
{
    if (!active_)
    {
        target = desired;
        return 0;
    }
    // 动力学、运动学计算
    if (!paused_)
    {
        std::lock_guard<std::mutex> lck(cmd_mutex_);
        if (!cmd_list_.empty())
        {
            last_user_cmd_rt_ = cmd_list_.front();
            cmd_list_.pop_front();
        }
        ServoData user_desired_pos = last_user_cmd_rt_;  // cmd_list_为空时，保持上一周的用户输入
        if (user_desired_pos.is_coord)
        {
            RobJointVal jointPos;
            ZucPose pose = user_desired_pos.cmd.pose;
            servo_filter_->do_cart_filt(pose);  // 无滤波器时返回原始值

            kine::KineItf::kine_Conf conf;
            if (kine_->ikine(pose, jointPos, conf, &tool, &user_frame) == 0)
            {
                if (axisgroup::ServoFilterIntf::FilterType::LPF_1ST == servo_filter_->get_filter_type())
                {
                    servo_filter_->do_joint_filter(jointPos.val);  // 无滤波器时返回原始值
                }
                user_desired_pos.cmd.jpos = jointPos;
                user_desired_pos.cmd.pose = pose;
            }
            else
            {
                SERVO_PLAN_ERR("ikine failed");
                return -1;
            }
        }
        else
        {
            servo_filter_->do_joint_filter(user_desired_pos.cmd.jpos.val);  // 无滤波器时返回原始值
            kine_->fkine(user_desired_pos.cmd.jpos, user_desired_pos.cmd.pose, &tool, &user_frame);
        }
        target = user_desired_pos.cmd;
        for (size_t i = 0; i < RobJointVal::MAX_AXIS; i++) { cmd_vel_[i] = (target.jpos[i] - last_cmd_rt_.jpos[i]) / ts_; }
        last_cmd_rt_ = target;
    }
    else
    {
        if (wish_to_stop_)
        {
            stop_planner_->init(last_cmd_rt_.jpos, cmd_vel_, RobJointVal::MAX_AXIS, ts_);
            wish_to_stop_ = false;
        }
        stop_planner_->update(target.jpos);
        SERVO_PLAN_DEBUG("Paused! target %s", target.jpos.val_str().c_str());
        kine_->fkine(target.jpos, target.pose, &tool, &user_frame);
        if (stop_planner_->is_stopped())
        {
            is_stopped_ = true;
            SERVO_PLAN_INFO("Stopped!");
            return 0;
        }
    }
    return 0;
}

void axisgroup::RtPlanServo::refine_pose(AxisGroupPose& cmd)
{
    if (cmd.pose.a - last_cmd_.pose.a > 180.0)
        cmd.pose.a = cmd.pose.a - 360.0;
    else if (cmd.pose.a - last_cmd_.pose.a < -180.0)
        cmd.pose.a = cmd.pose.a + 360.0;

    if (cmd.pose.b - last_cmd_.pose.b > 180.0)
        cmd.pose.b = cmd.pose.b - 360.0;
    else if (cmd.pose.b - last_cmd_.pose.b < -180.0)
        cmd.pose.b = cmd.pose.b + 360.0;

    if (cmd.pose.c - last_cmd_.pose.c > 180.0)
        cmd.pose.c = cmd.pose.c - 360.0;
    else if (cmd.pose.c - last_cmd_.pose.c < -180.0)
        cmd.pose.c = cmd.pose.c + 360.0;
}

int axisgroup::RtPlanServo::push_cmd(bool abs, int step_num, const AxisGroupPose& cmd_in)
{
    if (!active_)
    {
        SERVO_PLAN_ERR("servo plan is not active");
        return -1;
    }
    AxisGroupPose cmd = last_cmd_;
    std::vector<ServoData> cmd_list;

    if (mode_ == ServoMode::SERVO_COORD)  // 笛卡尔运动
    {
        /* STEP 1: normalize the pose orientation */
        if (!abs)  // 相对指令
        {
            cmd.pose.tran.x += cmd_in.pose.tran.x;
            cmd.pose.tran.y += cmd_in.pose.tran.y;
            cmd.pose.tran.z += cmd_in.pose.tran.z;
            PmRpy oldOri, deltOri, newOri;
            oldOri.y = cmd.pose.a * PM_PI / 180.0;
            oldOri.p = cmd.pose.b * PM_PI / 180.0;
            oldOri.r = cmd.pose.c * PM_PI / 180.0;
            deltOri.y = cmd_in.pose.a * PM_PI / 180.0;
            deltOri.p = cmd_in.pose.b * PM_PI / 180.0;
            deltOri.r = cmd_in.pose.c * PM_PI / 180.0;
            pmRpyRpyAdd(&oldOri, &deltOri, &newOri);
            cmd.pose.a = newOri.y * 180.0 / PM_PI;
            cmd.pose.b = newOri.p * 180.0 / PM_PI;
            cmd.pose.c = newOri.r * 180.0 / PM_PI;
        }
        else
        {
            cmd = cmd_in;
        }
        refine_pose(cmd);  // 姿态归到-180～180

        ServoData desired_pos;  // 用户的输入
        desired_pos.is_coord = true;
        // 线性插补
        /* STEP 2: consider the position command step */
        for (int stepIndex = 1; stepIndex <= step_num; stepIndex++)
        {
            desired_pos.cmd.pose.tran.x = last_cmd_.pose.tran.x + (cmd.pose.tran.x - last_cmd_.pose.tran.x) * stepIndex / step_num;
            desired_pos.cmd.pose.tran.y = last_cmd_.pose.tran.y + (cmd.pose.tran.y - last_cmd_.pose.tran.y) * stepIndex / step_num;
            desired_pos.cmd.pose.tran.z = last_cmd_.pose.tran.z + (cmd.pose.tran.z - last_cmd_.pose.tran.z) * stepIndex / step_num;
            desired_pos.cmd.pose.a = last_cmd_.pose.a + (cmd.pose.a - last_cmd_.pose.a) * stepIndex / step_num;
            desired_pos.cmd.pose.b = last_cmd_.pose.b + (cmd.pose.b - last_cmd_.pose.b) * stepIndex / step_num;
            desired_pos.cmd.pose.c = last_cmd_.pose.c + (cmd.pose.c - last_cmd_.pose.c) * stepIndex / step_num;
            cmd_list.emplace_back(desired_pos);
        }
    }
    else if (mode_ == ServoMode::SERVO_JOINT)  // 关节运动
    {
        size_t joint_num = 0;
        /* STEP 1: consider the position command step */
        for (int stepIndex = 1; stepIndex <= step_num; stepIndex++)
        {
            ServoData cmd_val;
            cmd_val.is_coord = false;
            if (!abs)
            {
                // 相对运动
                for (joint_num = 0; joint_num < RobJointVal::MAX_AXIS; ++joint_num)
                {
                    cmd_val.cmd.jpos[joint_num] = cmd.jpos[joint_num] + cmd_in.jpos.val[joint_num] * (stepIndex * 1.0 / step_num);
                }
            }
            else
            {
                for (joint_num = 0; joint_num < RobJointVal::MAX_AXIS; ++joint_num)
                {
                    double deltPos = cmd_in.jpos.val[joint_num] - cmd.jpos[joint_num];
                    cmd_val.cmd.jpos[joint_num] = cmd.jpos[joint_num] + deltPos * (stepIndex * 1.0 / step_num);
                }
            }
            cmd_list.push_back(cmd_val);  // 塞入指令
        }
    }
    last_cmd_ = cmd_list.back().cmd;

    {
        std::lock_guard<std::mutex> lck(cmd_mutex_);
        for (auto& cmd : cmd_list) { cmd_list_.push_back(cmd); }
    }
    return 0;
}
